

#include "Kinect.h"

#define CHECK_RC(rc, what)											\
	if (rc != XN_STATUS_OK)											\
	{						\
		printf("%s failed: %s\n", what, xnGetStatusString(rc));\
		return rc;													\
	}


Kinect::Kinect(int x)
{
	id = x;	//set id;
	HDC		m_hDC = NULL;
	HGLRC	m_hRC = NULL;

	m_view_far_clip = 3000.0f;
	m_view_near_clip = 5.0f;
	m_view_fov = 50.0f;
	//kinect Init
	g_bNeedPose = FALSE;
	strcpy(g_strPose, "");
	g_bDrawBackground = TRUE;
	g_bDrawPixels = TRUE;
	g_bDrawSkeleton = TRUE;
	g_bPrintID = TRUE;
	g_bPrintState = TRUE;
	g_bPause = false;
	g_bRecord = false;
	g_bQuit = false;
	nColors = 10;
}


Kinect::~Kinect(void)
{

}

int Kinect::getID() {
	return id;
}

/**
 *	init kinect device
 */
int Kinect::initKinect() {
	 XnStatus nRetVal = XN_STATUS_OK;
	 nRetVal = g_Context.InitFromXmlFile("Setup_XML.xml");
	 CHECK_RC(nRetVal, "InitFromXml");

	 /*find depth node and user node*/
	 nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator);
	 CHECK_RC(nRetVal, "Find depth generator");
	 nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator);
	 if (nRetVal != XN_STATUS_OK)
	 {
		 /*Creates a user generator*/
		 nRetVal = g_UserGenerator.Create(g_Context);
		 CHECK_RC(nRetVal, "Find user generator");
	 }
	 XnCallbackHandle hUserCallbacks, hCalibrationCallbacks, hPoseCallbacks;
	 if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON))
	 {
		 printf("Supplied user generator doesn't support skeleton\n");
		 return 1;
	 }
	 //register user event and skeleton recognition event handler
	 nRetVal = g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks);
	 //register calibration event handler
	 nRetVal = g_UserGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(UserCalibration_CalibrationStart, UserCalibration_CalibrationEnd, NULL, hCalibrationCallbacks);

	 /*if user generator needs pose for calibration, then register the related event handler*/
	 if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration())
	 {
		 g_bNeedPose = TRUE;
		 if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION))
		 {
			 printf("Pose required, but not supported\n");
			 return 1;
		 }
		 //register pose detection handler
		 nRetVal = g_UserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL, NULL, hPoseCallbacks);
		 nRetVal = g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose);
		 printf("g_strPose= %s\n", g_strPose);
	 }
	 //set what joints are active, ALL means all joints are active from head to feet
	 nRetVal = g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);

	 //Make sure all generators are generating data
	 nRetVal = g_Context.StartGeneratingAll();
	 CHECK_RC(nRetVal, "StartGenerating");
	 return 0;
}

void Kinect::updateBodyInfo() {
	xn::SceneMetaData sceneMD;
	xn::DepthMetaData depthMD;
	g_DepthGenerator.GetMetaData(depthMD);
	g_Context.WaitAndUpdateAll();
	g_DepthGenerator.GetMetaData(depthMD);
	g_UserGenerator.GetUserPixels(0, sceneMD);
	XnUserID aUsers[15];
	XnUInt16 nUsers = 15;
	g_UserGenerator.GetUsers(aUsers, nUsers);
	#define CONFIDENCE_LEVEL 0.5
	if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[0]))
	{
		body.bTracking=true;//we have user 1 tracking
		XnSkeletonJointPosition joint;
		g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[0], XN_SKEL_HEAD, joint);
		body.bHead = (joint.fConfidence >= CONFIDENCE_LEVEL);
		body.pHead = joint.position; 
		g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[0], XN_SKEL_NECK, joint);
		body.bNeck = (joint.fConfidence >= CONFIDENCE_LEVEL);
		body.pNeck = joint.position; 
		g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[0], XN_SKEL_LEFT_SHOULDER, joint);
		body.bLShoulder = (joint.fConfidence >= CONFIDENCE_LEVEL);
		body.pLShoulder = joint.position; 
		g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[0], XN_SKEL_RIGHT_SHOULDER, joint);
		body.bRShoulder = (joint.fConfidence >= CONFIDENCE_LEVEL);
		body.pRShoulder = joint.position; 
		g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[0], XN_SKEL_LEFT_ELBOW, joint);
		body.bLElbow = (joint.fConfidence >= CONFIDENCE_LEVEL);
		body.pLElbow = joint.position; 
		g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[0], XN_SKEL_RIGHT_ELBOW, joint);
		body.bRElbow = (joint.fConfidence >= CONFIDENCE_LEVEL);
		body.pRElbow = joint.position; 
		g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[0], XN_SKEL_LEFT_HAND, joint);
		body.bLHand = (joint.fConfidence >= CONFIDENCE_LEVEL);
		body.pLHand = joint.position; 
		g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[0], XN_SKEL_RIGHT_HAND, joint);
		body.bRHand = (joint.fConfidence >= CONFIDENCE_LEVEL);
		body.pRHand = joint.position; 
		g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[0], XN_SKEL_LEFT_HIP, joint);
		body.bLHip = (joint.fConfidence >= CONFIDENCE_LEVEL);
		body.pLHip = joint.position; 
		g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[0], XN_SKEL_RIGHT_HIP, joint);
		body.bRHip = (joint.fConfidence >= CONFIDENCE_LEVEL);
		body.pRHip = joint.position; 
		g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[0], XN_SKEL_LEFT_KNEE, joint);
		body.bLKnee = (joint.fConfidence >= CONFIDENCE_LEVEL);
		body.pLKnee = joint.position; 
		g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[0], XN_SKEL_RIGHT_KNEE, joint);
		body.bRKnee = (joint.fConfidence >= CONFIDENCE_LEVEL);
		body.pRKnee = joint.position;
		g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[0], XN_SKEL_LEFT_FOOT, joint);
		body.bLFoot = (joint.fConfidence >= CONFIDENCE_LEVEL);
		body.pLFoot = joint.position; 
		g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[0], XN_SKEL_RIGHT_FOOT, joint);
		body.bRFoot = (joint.fConfidence >= CONFIDENCE_LEVEL);
		body.pRFoot = joint.position;
		g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[0], XN_SKEL_TORSO, joint);
		body.bTorso = (joint.fConfidence >= CONFIDENCE_LEVEL);
		body.pTorso = joint.position;
	}
	else {
		body.bTracking = false;
	}
	//OpenGLDisplay();
}


BodyInfo Kinect::getBodyInfo() const {
	return body;
}

